import cv2
import numpy as np
import matplotlib.pyplot as plt
import serial
import binascii
import matplotlib.pyplot as plt
import math
import time
ser = serial.Serial()

def port_open():
    ser.port = 'com4'         #设置端口号
    ser.baudrate =115200     #设置波特率
    ser.bytesize = 8        #设置数据位
    ser.stopbits = 1        #设置停止位
    ser.parity = "N"        #设置校验位
    ser.open()              #打开串口,要找到对的串口号才会成功
    if(ser.isOpen()):
        print("打开成功")
    else:
        print("打开失败")
 
def port_close():
    ser.close()
    if (ser.isOpen()):
        print("关闭失败")
    else:
        print("关闭成功")
 
def send(send_data):
    if (ser.isOpen()):
        ser.write(send_data.encode('utf-8'))  #utf-8 编码发送
        #ser.write(binascii.a2b_hex(send_data))  #Hex发送
        print("发送成功",send_data)
    else:
        print("发送失败")
port_open() 

cap = cv2.VideoCapture(1)#捕获摄像头，0为默认摄像头，1为外接摄像

send('c+000250+1200d')
while True:
    ret, frame = cap.read()
    #显示图像窗口
    cv2.imshow("frame",frame) #显示视频图像
   
  
    if cv2.waitKey(1)==ord("a"):
       cv2.imwrite("./picture"+ str(1) + ".jpg",frame) #保存视频帧图片，自行修改路径
       cap.release()
       cv2.destroyAllWindows()
       break #按下“q”按键退出



def myplot(images,titles):   # For plotting multiple images at once
    fig, axs=plt.subplots(1,len(images),sharey=True)
    fig.set_figwidth(15)
    for img,ax,title in zip(images,axs,titles):
        if img.shape[-1]==3:
            img=cv2.cvtColor(img, cv2.COLOR_BGR2RGB) # OpenCV reads images as BGR, so converting back them to RGB
        else:
            img=cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
        ax.imshow(img)
        ax.set_title(title)

src_list=[]
def click_event(event, x, y, flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:
        print(x, ',',y)
        src_list.append((x,y))
        font = cv2.FONT_HERSHEY_SIMPLEX
        strXY = str(x) + ', '+ str(y)
        cv2.putText(jpg, strXY,(x,y), font, 0.3,(255,255,255),1)
        cv2.imshow('image', jpg)


img = cv2.imread("./picture"+ str(1) + ".jpg",1)  #读取图片
imgRef = cv2.imread("F:\opcv\picture"+ str(1) + ".png",1)  # 匹配模板图像 (matching template)
_, _, channel = img.shape
imgOut = np.zeros_like(img)
for i in range(channel):
        print(i)
        histImg, _ = np.histogram(img[:,:,i], 256)  # 计算原始图像直方图
        histRef, _ = np.histogram(imgRef[:,:,i], 256)  # 计算匹配模板直方图
        cdfImg = np.cumsum(histImg)  # 计算原始图像累积分布函数 CDF
        cdfRef = np.cumsum(histRef)  # 计算匹配模板累积分布函数 CDF
        for j in range(256):
            tmp = abs(cdfImg[j] - cdfRef)
            tmp = tmp.tolist()
            index = tmp.index(min(tmp))  # find the smallest number in tmp, get the index of this number
            imgOut[:,:,i][img[:,:,i]==j] = index

fig = plt.figure(figsize=(10,7))
plt.subplot(231), plt.title("Original image"), plt.axis('off')
plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))  # 显示原始图像
plt.subplot(232), plt.title("Matching template"), plt.axis('off')
plt.imshow(cv2.cvtColor(imgRef, cv2.COLOR_BGR2RGB))  # 显示匹配模板
plt.subplot(233), plt.title("Matching output"), plt.axis('off')
plt.imshow(cv2.cvtColor(imgOut, cv2.COLOR_BGR2RGB))  # 显示匹配结果
histImg, bins = np.histogram(img.flatten(), 256)  # 计算原始图像直方图
plt.subplot(234, yticks=[]), plt.bar(bins[:-1], histImg)
histRef, bins = np.histogram(imgRef.flatten(), 256)  # 计算匹配模板直方图
plt.subplot(235, yticks=[]), plt.bar(bins[:-1], histRef)
histOut, bins = np.histogram(imgOut.flatten(), 256)  # 计算匹配结果直方图
plt.subplot(236, yticks=[]), plt.bar(bins[:-1], histOut)
plt.show()

cv2.namedWindow('image',0)
jpg = imgOut
cv2.imshow('image',jpg)
cv2.setMouseCallback('image',click_event)
cv2.waitKey(0)
cv2.imwrite('./point_param.jpg',jpg)
cv2.destroyAllWindows()
scale_percent = 100       # 图片缩小比例
width = int(jpg.shape[1] * scale_percent / 100)
height = int(jpg.shape[0] * scale_percent / 100)
dim = (width, height)
jpg = cv2.resize(jpg, dim, interpolation = cv2.INTER_AREA) 
#图片尺寸过大会影响处理效率，所以进行缩放

print('Resized Dimensions : ',jpg.shape)
jpg = cv2.pyrMeanShiftFiltering(jpg,6,100)  #均值迁移，EPT边缘保留滤波
for i, pt in enumerate(src_list):
    
    cv2.circle(jpg, pt, 5, (0, 0, 255), -1) #cv2.circle根据给定的圆心和半径等画圆
    cv2.putText(jpg,str(i+1),(pt[0]+5,pt[1]+10),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
pts1 = np.float32(src_list) #原始图像四个顶点
pts2 = np.float32([[0, 0], [0, 480], [640, 480], [640, 0]]) #转换目标点
matrix = cv2.getPerspectiveTransform(pts1, pts2)
img = cv2.warpPerspective(jpg, matrix, (width,height)) #透视变换
cv2.imshow("Image", jpg)
cv2.resizeWindow("Image", 640,480)

hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV)  #HSV空间，图片为RGB格式
lower_red= np.array([100,43,46])   #设定阈值
upper_red = np.array([124,255,255])
red_mask = cv2.inRange(hsv,lower_red,upper_red)   #设定取值范围
red = cv2.bitwise_and(img,img,mask=red_mask)
contours=[]
count_while=0
while len(contours)!=6:
    dart = red.copy()            #复制图像
    kernel = np.ones((3,3),np.uint8)
    erode1=cv2.erode(red,kernel,dart,iterations=count_while)  #腐蚀运算
    

    img_gray=cv2.cvtColor(erode1,cv2.COLOR_BGR2GRAY)  #灰度，图片为RGB格式 
    _,img_binary=cv2.threshold(img_gray,25,255,cv2.THRESH_BINARY)  #二值化图像
    
    contours,hierarchy=cv2.findContours(img_binary,cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)   #边缘检测
    print(contours)
    print(len(contours))
    
    
    cv2.drawContours(img, contours,-1,(0,0,255),3) # Draws the contours on the original image just like draw function
    myplot([img_binary, img], ['Original Image', 'Contours in the Image'])
    dart0= contours[0]
    dart1= contours[1]
    dart2= contours[2]
    dart3= contours[3]
    dart4= contours[4]
    dart5= contours[5] 
    dart0 =  dart0[:,0]
    dart1 =  dart1[:,0]
    dart2 =  dart2[:,0]
    dart3 =  dart3[:,0]
    dart4 =  dart4[:,0]
    dart5 =  dart5[:,0]
    
    print(dart0.mean(axis = 0))  #取q平均值，获取坐标
    print(dart1.mean(axis = 0))
    print(dart2.mean(axis = 0))
    print(dart3.mean(axis = 0))
    print(dart4.mean(axis = 0))
    print(dart5.mean(axis = 0))
    print("\n translated to world axis \n")
    count_while+=1
myplot([red,erode1], ['org'+str(count_while),'dart'])

INPUT=[dart0.mean(axis = 0),dart1.mean(axis = 0),dart2.mean(axis = 0),dart3.mean(axis = 0),dart4.mean(axis = 0),dart5.mean(axis = 0)]
offset=np.array([0.,28.5])
Bili=np.array([-0.040,0.038])
strx=[]
stry=[]
cnt=0


#极坐标排序
TEMP=[]
count_TEMP=0
angle=[]
closeangles=[]#记录相近的极角
count_change=0#记录交换
count_changed_points=[]
count_change_points=[]
temp=0

# 获取列表的第二个元素
def takeSecond(elem):
    return elem[1]
def takeFirst(elem):
    return elem[0] 
def takeangle(elem):
    return math.atan2((elem[1]-240+offset[1]/Bili[1]),(elem[0]-320+offset[0]/Bili[0]))
# 指定第二个元素排序
INPUT.sort(key=takeangle,reverse=True)
'''
INPUT_temp=[[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]]
for i in INPUT:
     angle.append(takeangle(i))
for i in range(5):
    if(angle[i]-angle[i+1])<15/180*3.14:
        closeangles.append(i)
cot=0
for i in closeangles:
    if(i-temp)!=1:            
        INPUT_temp[count_change]=INPUT[i]
        INPUT_temp[5-count_change]=INPUT[i+1]
        count_change+=1
        count_changed_points.append(cot)
        count_changed_points.append(5-cot)
        count_change_points.append(i)
        count_change_points.append(i+1)
        temp=i
        cot+=1
for i in range(6):
    if i in count_change_points:
        print(str(i)+'change')
    else:
        TEMP.append(INPUT[i])
for i in range(6):
   if i in count_changed_points:
       print(str(i)+'changed')
   else:
         INPUT_temp[i]=TEMP[count_TEMP]
         count_TEMP+=1

print(INPUT)
print(INPUT_temp)
print(angle)
print(closeangles)
INPUT=INPUT_temp
'''
cmdpick=[]
cmdcilp=[]
for i in INPUT:
    out=(i-[320,240])*Bili+offset
    if out[0]>0:
        out+=[0.5,0]
    else:
        out+=[0.7,0]
    print(out)
    if(out[0]>10):
        strx.append("+"+str(int(out[0]*10)))
    else:
        if out[0]>1:
            strx.append("+0"+str(int(out[0]*10)))
        else:
            if out[0]>0:
                strx.append("+00"+str(int(out[0]*10)))
            else:
                if out[0]<-10:
                    strx.append("-"+str(abs(int(out[0]*10))))
                else:
                    if out[0]<-1:
                        strx.append("-0"+str(abs(int(out[0]*10))))
                    else:
                        strx.append("-00"+str(abs(int(out[0]*10))))
    if(out[1]>10):
        stry.append(str(int(out[1]*10)))
    else:
        if out[1]>1:
            stry.append("0"+str(int(out[1]*10)))
        else:
            if out[1]>0:
                stry.append("00"+str(int(out[1]*10)))
    cmdpick.append("c"+strx[cnt]+stry[cnt]+"+0101d")
    cmdcilp.append("c"+strx[cnt]+stry[cnt]+"+0100d")
    cnt+=1
cmdup='c+000250+1200u'
cmddown='c+000250+1001u'
print(strx)
print(stry)
cv2.drawContours(img,contours,-1,(0,0,255),3)
cv2.imshow("dart",erode1 )
cv2.imshow("red", red)  
cv2.imshow("img", img)  

cv2.imshow("blue", red) 
cv2.imshow("imgB", img_binary) 
cv2.waitKey(0)
send(cmdpick[0])
time.sleep(7)
send(cmdcilp[0])
time.sleep(7)
send(cmdup)
time.sleep(7)
time.sleep(7)
send(cmdcilp[5])
time.sleep(7)
send(cmdpick[5])
time.sleep(7)
send(cmddown)

time.sleep(7)
send(cmdpick[1])
time.sleep(7)
send(cmdcilp[1])
time.sleep(7)
send(cmdup)
time.sleep(7)
send(cmdcilp[4])
time.sleep(7)
send(cmdpick[4])
time.sleep(7)
send(cmddown)

time.sleep(7)
send(cmdpick[3])
time.sleep(7)
send(cmdcilp[3])
time.sleep(7)
send(cmdup)
time.sleep(7)
send(cmdcilp[2])
time.sleep(7)
send(cmdpick[2])
time.sleep(7)
send(cmddown)

port_close()




cv2.destroyAllWindows()  #销毁所有窗口
